#include <sstream>
#include "sys/time.h"
#include <math.h>
#include <stdio.h>
#include "serial.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <std_msgs/Float32.h>
#include <sstream>
#include "sys/time.h"
#include <math.h>
#include <stdio.h>
#include "serial.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <std_msgs/Float32.h>

#include <iostream>

int initsecs;
int fd;

double getTime()
{
    struct timeval t;
    gettimeofday(&t,NULL);
    double rt=t.tv_usec;
    rt=rt/1000000+t.tv_sec;
    return rt;
}

inline short getRandNum(int a,int b){
    return (short)(rand()%(b-a+1)+a);
}


int main(int argc, char **argv) {
	ros::init(argc, argv, "xbee_sar_com1");
	ros::NodeHandle n;
    ros::Publisher send_pub = n.advertise<std_msgs::Float32>("/xbee/send_com1", 100);
	ros::Publisher receive_pub = n.advertise<std_msgs::Float32>("/xbee/receive_com1", 100);
    fd = serial_open_file("/dev/ttyUSB0", 57600,0);
    ROS_INFO("Open Serial: [%d]", fd);
    while(fd<0){
    fd = serial_open_file("/dev/ttyUSB0", 57600,0);
    ROS_INFO("Open Serial: [%d]", fd);
    }
    int timeout_set_status = serial_set_timeout(fd,0); 
    double begin_time;
    double last_time;
    double time_interval;
    short send_status;
    short receive_status;
    std_msgs::Float32 xbee_data_out2, xbee_data_in2;
    double t_index = 0;
    ros::Rate loop(50);
    while (ros::ok()){
        short xbee_data_out[14];
        short xbee_data_in[14];
        int secs[2];
        static long temp = 0;
        temp ++;
        xbee_data_out2.data = temp % 1000;
        t_index += 1.0/100.0f;
        xbee_data_out[0]=(short)(0.1 *10000);
        xbee_data_out[1]=(short)(0.02*10000);
        xbee_data_out[2]=(short)(xbee_data_out2.data);
        xbee_data_out[3]=(short)(getRandNum(0,10000));
        xbee_data_out[4]=(short)(0.05*10000);
        xbee_data_out[5]=(short)(-1*10000);
        xbee_data_out[6]=(short)(-1*10000);
        xbee_data_out[7]=(short)(-1*10000);
        xbee_data_out[8]=(short)(-1*10000);
        xbee_data_out[9]=(short)(-1*10000);
        xbee_data_out[10]=(short)(-1*10000);
        if(fd<1)
        {
            fd = serial_open_file("/dev/ttyUSB0", 57600,0);
        }
        begin_time = getTime();
        send_status = sendmessage(fd,xbee_data_out);
        receive_status = readmessage(fd,xbee_data_in);
        // if (receive_status == 0){
        //     continue;
        // }
        last_time = getTime();
        time_interval = last_time-begin_time;
        ROS_INFO("------COM1----------\n"
                 "uart_status:[%d],time_interval:[\t%4.4f]\n"
                 "send_status:[%d],data:[\t%5d,\t%5d,\t%5d,\t%5d]\n"
                 "receive_status:[%d],data:[\t%5d,\t%5d,\t%5d,\t%5d]\n"
                 "======================================================\n",
        (int)fd,
        (double)time_interval,
        (int)send_status,
        (int)xbee_data_out[0],
        (int)xbee_data_out[1],
        (int)xbee_data_out[2],
        (int)xbee_data_out[3],
        (int)receive_status,
        (int)xbee_data_in[0],
        (int)xbee_data_in[1],
        (int)xbee_data_in[2],
        (int)xbee_data_in[3]
        );
        //xbee_data_in2.data = (float)xbee_data_in[2]/10000.0f;
        xbee_data_in2.data = (float)xbee_data_in[2];
        send_pub.publish(xbee_data_out2);
        receive_pub.publish(xbee_data_in2);
        loop.sleep();
        // ros::spinOnce();
    }
	return 0;
}
